from cmath import atan, nan, sqrt
from itertools import count
from re import X

import cv2
import numpy as np
import serial


def findLine(img0):
    # img=cv2.imread("runpy.jpg")
    # img0=cv2.imread("runpy.jpg")
    # img0=cv2.imread(frameSeries[photoCnt])
    gray = cv2.cvtColor(img0, cv2.COLOR_BGR2GRAY)
    blur = cv2.GaussianBlur(gray, (5, 5), 0)
    blur = blur.astype("uint8")
    ret, th = cv2.threshold(blur, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
    th = cv2.dilate(th, None, iterations=2)
    th = cv2.erode(th, None, iterations=6)
    edges = cv2.Canny(th, 70, 80, apertureSize=3)
    # cv2.imshow('otsu',edges)

    # Hough
    # lines=cv2.HoughLinesP(edges,0.01,np.pi/180.0,10,minLineLength=30,maxLineGap=50)
    # for x1,y1,x2,y2 in lines[0]:
    #    cv2.line(img,(x1,y1),(x2,y2),(0,255,0),2)

    # LSD
    # lsd=cv2.createLineSegmentDetector(0)
    # dlines=lsd.detect(edges)
    # for dline in dlines[0]:
    #    x0=int(round(dline[0][0]))
    #    y0=int(round(dline[0][1]))
    #    x1=int(round(dline[0][2]))
    #    y1=int(round(dline[0][3]))
    #    cv2.line(img,(x0,y0),(x1,y1),(0,255,0),1,cv2.LINE_AA)

    # FLD
    fld = cv2.ximgproc.createFastLineDetector()
    dlines = fld.detect(edges)
    # drawn_img=fld.drawSegments(img,dlines,)
    for dline in dlines:
        x0 = int(round(dline[0][0]))
        y0 = int(round(dline[0][1]))
        x1 = int(round(dline[0][2]))
        y1 = int(round(dline[0][3]))
        cv2.line(img0, (x0, y0), (x1, y1), (0, 255, 0), 4, cv2.LINE_AA)
    # 1944  2592
    # cv2.imshow('lines',img)

    grayl = cv2.cvtColor(img0, cv2.COLOR_BGR2GRAY)
    blurl = cv2.GaussianBlur(grayl, (5, 5), 0)
    blurl = blurl.astype("uint8")
    retl, thl = cv2.threshold(blurl, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
    thl = cv2.dilate(thl, None, iterations=2)
    thl = cv2.erode(thl, None, iterations=6)
    # cv2.imshow('ostu',thl)
    edgesl = cv2.Canny(thl, 70, 80, apertureSize=3)
    # cv2.imshow('img',edgesl)

    # HoughLineP Algorithm
    # linesl = cv2.HoughLinesP(edgesl, 3, np.pi / 180, 50, minLineLength=50, maxLineGap=100)
    # for line in linesl:
    #     x1, y1, x2, y2 = line[0]
    #     # x1 = line[0][0]
    #     # y1 = line[0][1]
    #     # x2 = line[0][2]
    #     # y2 = line[0][3]
    #     cv2.line(img, (x1, y1), (x2, y2), (255, 255, 0), 2)
    # cv2.imshow('lines',img)

    # HoughLine Algorithm
    # linesl = cv2.HoughLines(edgesl, 1, np.pi / 180, 100)
    # for line in linesl:
    #     rho = line[0][0]
    #     theta = line[0][1]
    #     a = np.cos(theta)
    #     b = np.sin(theta)
    #     x0 = a * rho
    #     y0 = b * rho
    #     x1 = int(x0 + 1000 * (-b))
    #     y1 = int(y0 + 1000 * (a))
    #     x2 = int(x0 - 1000 * (-b))
    #     y2 = int(y0 - 1000 * (a))
    #     cv2.line(img, (x1, y1), (x2, y2), (0, 0, 255), 2)
    # cv2.imshow('img1', img)

    # 左右寻线法
    # Search the line on the right side
    # search from the middle of the img, the range is 2/3 of the height
    # img_array=np.array(edgesl)
    # shape=img_array.shape
    # print(shape)
    # height=shape[0]
    # width=shape[1]
    # pixW_right=[]
    # midPixH_right=[]
    # wMid=round(width/2)
    # searchHeight=round(1/3*height)
    # for midPixH in range(height-1,searchHeight,-1):
    #     for pixW in range(wMid,width-1,1):
    #         (b,g,r)=img_array[midPixH,pixW]
    #         if (b,g,r)==(0,255,0):
    #             #print(pixW,midPixH)
    #             pixW_left.append(pixW)
    #             midPixH_left.append(midPixH)
    #             break

    # Search the middle pixel of the white part
    img_array = np.array(thl)
    shape = img_array.shape
    bottomMid = int(shape[1]/2)
    searchHeightThr = int(shape[0]/8)
    trackLeftSeed = []
    flagLeftSeed = 0
    trackRightSeed = []
    flagRightSeed = 0
    for searchHeight in range(shape[0]-1, searchHeightThr, -1):
        for trackLeft in range(bottomMid, 0, -1):
            gray = img_array[searchHeight][trackLeft]
            if gray == 0:
                trackLeftSeed = (searchHeight, trackLeft)
                flagLeftSeed = 1
                break
        if flagLeftSeed == 1:
            break

    for searchHeight0 in range(shape[0]-1, searchHeightThr, -1):
        for trackRight in range(bottomMid, shape[1]-1, 1):
            gray = img_array[searchHeight0][trackRight]
            if gray == 0:
                trackRightSeed = (searchHeight0, trackRight)
                flagRightSeed = 1
                break
        if flagRightSeed == 1:
            break

    print(trackLeftSeed)
    print(trackRightSeed)

    # 左侧赛道 Ltrack
    Ltrack = []
    xvalueL = trackLeftSeed[1]
    grayscaleL = nan
    for i in range(trackLeftSeed[0], searchHeightThr, -1):
        for j in range(xvalueL, shape[1], 1):
            # 横坐标从本行向上寻找一行，再向右寻边
            grayscaleL = img_array[i][j]
            if grayscaleL == 255:
                xvalueL = j-1
                Ltrack.append((i, j-1))
                break

    print(Ltrack[-1])

    # 右侧赛道 RTrack
    Rtrack = []
    xvalueR = trackRightSeed[1]
    grayscaleR = nan
    for k in range(trackRightSeed[0], searchHeightThr, -1):
        for l in range(xvalueR, 0, -1):
            # 横坐标从本行向上寻找一行，再向左寻边
            grayscaleR = img_array[k][l]
            if grayscaleR == 255:
                xvalueR = l+1
                Rtrack.append((k, l+1))
                break

    print(Rtrack[-1])
    # 寻找左右赛道相对应的点，绘制中线
    # 寻找横坐标相等的左右对应点
    # 先比较对应点的纵坐标左右哪一个高，从较高的一边开始对应
    flagRight = 0
    if trackRightSeed[0] >= trackLeftSeed[0]:  # 右边y大于左边，右边较低，与左侧对齐
        pass
    else:
        flagRight = 1

    # 将较低的list没有对应的地方跳过，到两边横坐标相等
    # 本段代码只用首尾两点找中线，适合直线
    index = 0
    if flagRight == 0:
        index = len(Rtrack)-len(Ltrack)
        midPixBottomx = int(
            ((Rtrack[index])[1]-(Ltrack[0])[1])/2+(Ltrack[0])[1])
        midPixBottomy = Ltrack[0][0]
        midPixTopx = int(((Rtrack[-1])[1]-(Ltrack[-1])[1])/2+(Ltrack[-1])[1])
    else:
        index = len(Ltrack)-len(Rtrack)
        midPixBottomx = int(
            ((Rtrack[0])[1]-(Ltrack[index])[1])/2+(Ltrack[index])[1])
        midPixBottomy = Rtrack[0][0]
        midPixTopx = int(((Rtrack[-1])[1]-(Ltrack[-1])[1])/2+(Ltrack[-1])[1])

    midPixBottom = (midPixBottomx, midPixBottomy)
    midPixTop = (midPixTopx, searchHeightThr)
    print(midPixBottom)
    print(midPixTop)
    line = (midPixBottom, midPixTop)
    print(line)
    cv2.line(thl, midPixBottom, midPixTop, 0, 3)
    cv2.imshow('line', thl)
    return line

# 计算直线斜率


def slope(line):
    k = (line[0][1]-line[1][1])/(line[1][0]-line[0][0])
    return k

# 计算两条线中点间距，作为距离度量，单位是pixel


def distance(line1, line2):
    midPoint1 = (int((line1[0][0]+line1[1][0])/2),
                 int((line1[0][1]+line2[1][1])/2))
    midPoint2 = (int((line2[0][0]+line2[1][0])/2),
                 int((line2[0][1]+line2[1][1])/2))
    dis = sqrt(pow((midPoint1[0]-midPoint2[0]), 2) +
               pow((midPoint1[1]-midPoint2[1]), 2))
    return dis

# 计算两条线之间的夹角,返回值为弧度


def angle(line1, line2):
    slope1 = slope(line1)
    slope2 = slope(line2)
    agl = atan((slope2-slope1)/(1+slope1*slope2))
    return agl


cv2.waitKey()
cv2.destroyAllWindows()
